#include "GameMotionState.h"
#include "LinearMath\btMatrix3x3.h"

//-------------------------------------------------------------------------------------
GameMotionState::GameMotionState(const btTransform &pos, ObjectPtr &obj, const btVector3 &offset)
{
	mPosition = pos;
	mpBaseObject = obj;
	mOffset = vecToMat(offset);
}
//-------------------------------------------------------------------------------------
GameMotionState::GameMotionState(ObjectPtr &object, const btVector3 &offset)
{
	mOffset = vecToMat(offset);
	BaseObjectPtr obj = boost::static_pointer_cast<BaseObject>(object);
	Ogre::Matrix4 orien = obj->getNode()->getOrientation();
	btVector3 pos(obj->getNode()->getPosition().x,obj->getNode()->getPosition().y,obj->getNode()->getPosition().z);
	btQuaternion quat(orien.extractQuaternion().x,
		orien.extractQuaternion().y,
		orien.extractQuaternion().z,
		orien.extractQuaternion().w);
	mPosition = btTransform(quat,pos);

	mpBaseObject = obj;
}
//-------------------------------------------------------------------------------------
GameMotionState::~GameMotionState(void)
{
}
//-------------------------------------------------------------------------------------
ObjectPtr &GameMotionState::getObject()
{
	return mpBaseObject;
}
//-------------------------------------------------------------------------------------
void GameMotionState::getWorldTransform(btTransform &trans) const
{
	trans = mPosition;
}
//-------------------------------------------------------------------------------------
void GameMotionState::setWorldTransform(const btTransform &trans)
{
	BaseObjectPtr obj = boost::static_pointer_cast<BaseObject>(mpBaseObject);
	btTransform off = trans * mOffset;
	btQuaternion rot = off.getRotation();
	obj->getNode()->setOrientation(rot.w(), rot.x(), rot.y(), rot.z());
	btVector3 pos = off.getOrigin();
	obj->getNode()->setPosition(pos.x(), pos.y(), pos.z());
}
//-------------------------------------------------------------------------------------
btTransform GameMotionState::vecToMat(const btVector3& vec)
{
	btMatrix3x3 mat(1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0);
	return btTransform(mat, vec);
}
//-------------------------------------------------------------------------------------
btTransform GameMotionState::getOffset(void)
{
	return mOffset;
}